

#include "object.h"

#include "vector4.h"
#include "matrix44.h"

Object::Object(void)
{
	tMat =  Matrix44();
	pos = Vector4();
	
	id = 0;
}
Object::Object(float x, float y, float z)
{
	tMat =  Matrix44();
	id = 0;

	translateXYZ(x,y,z);
}
Object::Object(float x, float y, float z,
	   float dx,float dy,float dz)
{
	tMat = Matrix44();
	id = 0;

	translateXYZ(x,y,z);

	vel = Vector4(dx, dy, dz);
}


void Object::update(float etime)
{
	Vector4 emptyPos = Vector4();
	RK4(emptyPos, vel, acc, etime);
	translateXYZ(emptyPos[0], emptyPos[1], emptyPos[2]);
}


void Object::translateXYZ(float x, float y, float z){
	pos = pos + Vector4(x,y,z);
	//tMat = tMat.translateXYZ(x, y, z);
}

void Object::scale(float s){
	sc = sc * s;
}

void Object::EulerInt(Vector4 &position, Vector4 &velocity, Vector4 &acceleration, float dt)
{
	for(int i=0; i<3; i++){
		position[i] = (0.5f*acceleration[i]*dt*dt) + (velocity[i]*dt) + position[i];
		velocity[i] = (acceleration[i]*dt) + velocity[i];
	}
}

void Object::RK4(Vector4 &position, Vector4 &velocity, Vector4 &acceleration, float dt)
{
	for(int i = 0; i < 3; i++){
		//k1 component
		float x1 = position.get(i);
        float v1 = velocity.get(i);
        float a1 = acceleration.get(i);
        //k2 component
        float x2 = position.get(i) + .5f*v1*dt;
        float v2 = velocity.get(i) + .5f*a1*dt;
        float a2 = acceleration.get(i);
        //k3 component
        float x3 = position.get(i) + .5f*v2*dt;
        float v3 = velocity.get(i) + .5f*a2*dt;
        float a3 = acceleration.get(i);
        //k4 component
        float x4 = position.get(i) + v3*dt;
        float v4 = velocity.get(i) + a3*dt;
        float a4 = acceleration.get(i);
        //weighted average gives new position and velocity
        position.set(i, position.get(i) + (dt/6.f)*(v1 + 2.f*v2 + 2.f*v3 + v4));
        velocity.set(i, velocity.get(i) +(dt/6.f)*(a1 + 2.f*a2 + 2.f*a3 + a4));
		//std::cout << position[0] << " " <<position[1] << " " <<position[2] << std::endl;;
	}
}